private double calcularCosteheuristica(String heuristica, Posicion posActual, Posicion posFin){
	double res=0;
	if (heuristica.equals("euclidea")){
		res=Math.sqrt(Math.pow(posActual.getX()-posFin.getX(), 2)) + Math.sqrt(Math.pow(posActual.getY()-posFin.getY(), 2));
		res*=calcularCoste(1,1);
	}else if (heuristica.equals("manhattan")){
		res=Math.abs(posActual.getX()-posFin.getX()) + Math.abs(posActual.getY()-posFin.getY());
		res*=calcularCoste(1,1);
	}
	return res;
}

private Camino busquedaA(String heuristica, int[][] mapa, Posicion posIni, Posicion posFin){
	Camino resOptimo = new Camino();
	Camino res = new Camino();
	boolean solucionEncontrada = false;

	/* Inicializamos la solucion optima con el peor valor */
	resOptimo.setC(500.0);
	/* Anadimos el primer nodo como candidato */
	Nodo n = new Nodo(posIni, 0, 0, 0, 0, null);
	n.setDireccion(1);	/* Escogemos horizontal como la primera direccion */
	monticulo.add(n);
	
	/* Definimos el nodo error. Este indicara que quedan candidatos, pero que no son
	 * de la misma profundidad que la actual y tendremos que aumentarla. Es decir,
	 * se han comprobado todos los nodos de un nivel.
	 */
	Nodo error=new Nodo(new Posicion(-1,-1),0,0,0,0,null);
	
	Nodo [] copiasCandidato = new Nodo[4];
	int profundidadActual = 0;
	double maxNodos = 0;
	/* Comenzamos el bucle de busqueda */
	while (!solucionEncontrada) {
		n = seleccionarCandidatoP();
		if (n != null) { /* Hay candidatos */
			if (n.equals(error)){ /* Hemos comprobado todos los candidatos de un nivel */
				profundidadActual++; /* Pasamos al nivel siguiente */
			}else{
				if (n.getPos().equals(posFin)) { /* Si es el nodo objetivo */
					/* Ponemos una pieza de avanzar simple y hemos encontrado una solucion */
					n.setPieza(1);
					n.setCoste(n.getPadre().getCoste() + calcularCoste(mapa[(int) n.getPos().getX()][(int) n.getPos().getY()], 1));
					res=construirSolucion(n);
					n.getVisitados().add(posFin);
					/* Calculamos el coste de la solucion y el numero maximo de nodos almacenados */
					res.setC(n.getCoste());
					res.setM(maxNodos);
					/* Comprobamos si es la mejor solucion */
					if (res.getC() < resOptimo.getC())
						resOptimo=res;
				} else { /* Si no es el nodo objetivo */
					
					for (int i=1;i<5;i++){ /* Probamos todas las piezas */
						/* Hacemos una copia del candidato para que no se sobrescriban las piezas */
						copiasCandidato[i-1]= new Nodo(n.getPos(),0,0,n.getF(),n.getProfundidad(),n.getPadre());
						copiasCandidato[i-1].setDireccion(n.getDireccion());
						if(copiasCandidato[i-1].getPadre() != null){  /* Pixel intermedio */
							copiasCandidato[i-1].setPieza(i);
							copiasCandidato[i-1].setCoste(copiasCandidato[i-1].getPadre().getCoste()+ calcularCoste(mapa[(int) copiasCandidato[i-1].getPos().getX()][(int) copiasCandidato[i-1].getPos().getY()], copiasCandidato[i-1].getPieza()));
						} else { /* Pixel inicial */
							if(i==1 || i==2){
								copiasCandidato[i-1].setPieza(i);
								copiasCandidato[i-1].setCoste(calcularCoste(mapa[(int) copiasCandidato[i-1].getPos().getX()][(int) copiasCandidato[i-1].getPos().getY()], copiasCandidato[i-1].getPieza()));
							}
						}
						copiasCandidato[i-1].getVisitados().add(copiasCandidato[i-1].getPos());
						/* Si el coste aun es menor que el de la mejor solucion */
						if (copiasCandidato[i-1].getCoste()<resOptimo.getC())
							generarCandidatos(copiasCandidato[i-1], copiasCandidato[i-1].getDireccion(),profundidadActual,
									copiasCandidato[i-1].getCoste()+calcularCosteheuristica(heuristica,copiasCandidato[i-1].getPos(), posFin));
									/* Como es busqueda A* le pasamos el coste acumulado hasta este nodo mas el coste
									 * de la distancia hasta el objetivo segun la heuristica */
						else /* Si no */
							/* Se abandona este camino*/ ;
					}
					if (maxNodos < monticulo.size())
						maxNodos = monticulo.size();
				}
			}
		}else{ /* Si no hay candidatos */
			solucionEncontrada=true;
			if (!resOptimo.getCamino().isEmpty())
				System.out.println("Se ha encontrado Solucion");
			else
				System.out.println("No se ha encontrado Solucion");
		}
	}

	return resOptimo;
}
